
#include "member_BHingeRestraint.h"
#include "member_BRestraint.h"
#include "member_BNode.h"

using namespace BWE;

member_BHingeRestraint::member_BHingeRestraint(BHingeRestraint* restraint)
{
	boss = restraint;

	axisA.set(1, 0, 0);
	axisB.set(1, 0, 0);

	hingeAngle = 0;
	solveLimit = false;
	angleError = 0;

	motorEnabled = false;
	motorReached = false;
	motorTarget = 0;
	motorVelocity = 0;
	motorMaxForce = 0;
}
member_BHingeRestraint::~member_BHingeRestraint()
{

}

void member_BHingeRestraint::caculate()
{
	const BBody* bodyA = restraint_member(boss)->bodyA;
	const BBody* bodyB = restraint_member(boss)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	BMatrix frameA = BQuater(BVector(1, 0, 0), axisA.normal());
	BMatrix frameB = BQuater(BVector(1, 0, 0), axisB.normal());
	dirAxis = matrixA.mult3(frameA.row(0));
	horAxis = matrixA.mult3(frameA.row(1));
	verAxis = matrixA.mult3(frameA.row(2));
	swingAxis = matrixB.mult3(frameB.row(1));
	BReal Y = swingAxis.dot(verAxis);
	BReal X = swingAxis.dot(horAxis);
	hingeAngle = atan2(Y, X);

	angleError = 0;
	solveLimit = false;
	if (angularLimit.valid())
	{
		hingeAngle = LimitAngle(hingeAngle, angularLimit.min(), angularLimit.max());
		if (hingeAngle < angularLimit.min())
		{
			angleError = angularLimit.min() - hingeAngle;
			solveLimit = true;
		}
		else if (hingeAngle > angularLimit.max())
		{
			angleError = angularLimit.max() - hingeAngle;
			solveLimit = true;
		}
	}
}
BReal member_BHingeRestraint::motorFactor(BReal stepTime)
{
	BReal value = hingeAngle;
	BReal target = motorTarget;
	if (abs(target) > -PI2 && abs(target) < PI2 && abs(target) > PI / 2)
	{
		if (target < 0)
			target += PI2;
		if (value < 0)
			value += PI2;
	}
	BReal factor = 1;
	BReal delta = abs(motorVelocity) * stepTime;
	BReal limit = angularLimit.size();
	if (!motorReached && limit > REAL_EPSILON)
	{
		if (abs(target - value) < delta)
		{
			motorReached = true;
			boss->emit(Signal_AngularMotorReached, true);
		}
	}
	if (motorReached)
	{
		if (motorVelocity > REAL_EPSILON)
		{
			if (value < target)
			{
				if (target - value < delta)
					factor = (target - value) / delta;
				else
					factor = 1;
			}
			else
			{
				if (value - target < delta)
					factor = (target - value) / delta;
				else
					factor = -1;
			}
		}
		if (motorVelocity < -REAL_EPSILON)
		{
			if (value > target)
			{
				if (value - target < delta)
					factor = (value - target) / delta;
				else
					factor = 1;
			}
			else
			{
				if (target - value < delta)
					factor = (value - target) / delta;
				else
					factor = -1;
			}
		}
	}
	return factor;
}
